package edu.wpi.first.wpilibj.util;

import java.util.Collection;
import java.util.Iterator;
import java.util.NoSuchElementException;
import java.util.Queue;

public class ArrayQueue<T> implements Queue<T> {

    private T[] objectArray;
    private int bottomIndex;
    private int topIndex = -1;
    
    public ArrayQueue(){
        this (8);
    }

    @SuppressWarnings("unchecked")
    public ArrayQueue(int capacity) {
        this.objectArray = (T[]) new Object[capacity];
    }
    
    

    @Override
    public boolean add(T e) {
        if (contains (e)){return false;}
        if (++topIndex == objectArray.length) {
            topIndex = 0;
        }
        if (topIndex == bottomIndex) {
            resize(objectArray.length + (int) (0.25 * objectArray.length) + 1);
        }
        objectArray[topIndex] = e;
        return true;
    }

    @Override
    public T element() {
        if (isEmpty()) {
            throw new NoSuchElementException("Queue is empty.");
        }
        return peek();
    }

    @Override
    public boolean offer(T e) {
        if (contains (e)){return true;}
        if (++topIndex == objectArray.length) {
            topIndex = 0;
        }
        if (topIndex == bottomIndex) {
            return false;
        }
        objectArray [topIndex] = e;
        return true;
    }

    @Override
    public T peek() {
        return objectArray[bottomIndex];
    }

    @Override
    public T poll() {
        T t = objectArray[bottomIndex];
        if (t == null) {
            return null;
        }
        objectArray[bottomIndex] = null;
        bottomIndex++;
        if (bottomIndex == objectArray.length) {
            bottomIndex = 0;
        }
        return t;
    }

    @Override
    public T remove() {
        if (isEmpty()) {
            throw new NoSuchElementException("Queue is empty");
        }
        return poll();
    }

    @Override
    public boolean addAll(Collection<? extends T> c) {
        boolean changed = false;
        for (Iterator<? extends T> it = c.iterator(); it.hasNext();) {
            T next = it.next();
            if (contains(next)) {
                continue;
            }
            changed = true;
            add(next);
        }
        return changed;
    }

    @Override
    @SuppressWarnings("unchecked")
    public void clear() {
        objectArray = (T[]) new Object[objectArray.length];
        bottomIndex = 0;
        topIndex = 0;
    }

    @Override
    public boolean contains(Object o) {
        for (int x = bottomIndex; x < topIndex; x++) {
            System.out.println (bottomIndex + " " + topIndex + " " + x);
            if (x == objectArray.length) {
                x = 0;
            }
            if (objectArray[x].equals(o)) {
                return true;
            }
        }
        return false;
    }

    @Override
    public boolean containsAll(Collection<?> c) {
        boolean b = true;
        for (Iterator <?> it = c.iterator();it.hasNext();){
            Object next = it.next();
            b = b && contains (next);
            if (!b){return false;}
        }
        return true;
    }

    @Override
    public boolean isEmpty() {
        return size() <= 0;
    }

    @Override
    @SuppressWarnings("unchecked")
    public Iterator<T> iterator() {
        final T[] copiedarray = (T[]) toArray();
        return new Iterator<T>() {
            
            private int currentIndex;

            @Override
            public boolean hasNext() {
                return currentIndex < copiedarray.length;
            }

            @Override
            public T next() {
                if (!hasNext()){
                    throw new NoSuchElementException();
                }
                return copiedarray [currentIndex++];
            }

            @Override
            public void remove() {
                currentIndex++;
            }
        };
    }

    @Override
    public boolean remove(Object o) {
        System.err.println (o.toString());
        for (int x = bottomIndex; x < topIndex; x++) {
            System.err.println (objectArray [x]);
            if (x == objectArray.length) {
                if (topIndex == 0) {
                    break;
                }
                x = 0;
            }
            if (objectArray[x].equals(o)) {
                movenull (x);
                return true;
            }
        }
        return false;
    }

    @Override
    public boolean removeAll(Collection<?> c) {
        boolean b = false;
        for (Iterator <?> it = c.iterator(); it.hasNext();){
            b = b || remove (it.next());
        }
        return b;
    }

    @Override
    public boolean retainAll(Collection<?> c) {
        boolean b = false;
        for (Iterator <T> it = iterator(); it.hasNext();){
            T next = it.next();
            if (!c.contains(next)){
                b = true;
                remove (next);
            }
        }
        return b;
    }

    @Override
    public int size() {
        if (topIndex >= bottomIndex) {
            return topIndex - bottomIndex + 1;
        } else {
            return objectArray.length - (bottomIndex - topIndex + 1);
        }
    }

    @Override
    public Object[] toArray() {
        if (isEmpty()){
            return new Object[0];
        }
        Object[] o = new Object[size()];
        System.arraycopy(objectArray, bottomIndex, o, 0, size());
        return o;
    }

    @Override
    @SuppressWarnings("unchecked")
    public <T> T[] toArray(T[] a) {
        System.arraycopy(objectArray, bottomIndex, a, 0, size());
        return a;
    }

    @SuppressWarnings("unchecked")
    private void resize(int newsize) {
        int size = size();
        T[] oldarray = objectArray;
        objectArray = (T[]) new Object[newsize];
        System.arraycopy(oldarray, bottomIndex, objectArray, 0, size);
    }
    
    private static String toString (Object[] o){
        StringBuilder sb = new StringBuilder("[");
        for (int x = 0; x < o.length; x++){
            sb.append(x).append(" : ").append(o[x]).append(", ");
        }
        sb.deleteCharAt(sb.length() - 1);
        sb.deleteCharAt(sb.length() - 1);
        sb.append("]");
        return sb.toString();
    }
    
    @SuppressWarnings("unchecked")
    private void movenull(int index){
//      System.err.println (index + " " + topIndex++ + " " + objectArray [index] + " " + java.util.Arrays.toString (objectArray));
        T[] oldarray = (T[]) new Object[objectArray.length];
        System.arraycopy(objectArray, 0, oldarray, 0, objectArray.length);
//      System.err.printf ("%s %d %s %d %d\n", java.util.Arrays.toString (java.util.Arrays.copyOfRange(oldarray, index + 1, index + 1 + (topIndex > index ? topIndex - index - 1 : objectArray.length - index - 1))), index + 1, java.util.Arrays.toString (java.util.Arrays.copyOfRange(objectArray, index, index + (topIndex > index ? topIndex - index - 1 : objectArray.length - index - 1))), index, topIndex > index ? topIndex - index - 1 : objectArray.length - index - 1);
        int max = topIndex > index ? topIndex : oldarray.length - 1;
        for (int x = max;x > index;x--){
            objectArray [x - 1] = oldarray [x];
            objectArray [x] = null;
        }
        if (topIndex < index){
            objectArray [objectArray.length - 1] = objectArray [0];
            System.arraycopy(oldarray, 1, objectArray, 0, topIndex - 1);
        }
        topIndex--;
        if (topIndex < 0){
            topIndex = objectArray.length - 1;
        }
    }
    
}
